#include "Gesture.h"
#include "macros.h"
#include <cmath>
#include <iostream>

using namespace std;

Gesture::Gesture()
{
	this->leftHandUpState = fail;
	this->rightHandUpState = fail;
	this->twoHandUpState = fail;
}

Gesture::~Gesture()
{
}

void Gesture::processGesture( NUI_SKELETON_DATA* currentSkeleton )
{
	proccessRightHandUpGesture(currentSkeleton);
	proccessLeftHandUpGesture(currentSkeleton);
	proccessTwoHandUpGesture(currentSkeleton);


	//cout << getLeftShoulderHandAngle(currentSkeleton) << " " << getRightShoulderHandAngle(currentSkeleton) << endl;
}

void Gesture::proccessRightHandUpGesture( NUI_SKELETON_DATA* currentSkeleton )
{
	double angle = getRightShoulderHandAngle(currentSkeleton);

	if(angle != -200)
	{
		switch(rightHandUpState)
		{
		case fail:
			if(angle > -40 && angle < -20)
				rightHandUpState = check1;
			break;
		case check1:
			if(angle > -10 && angle < 10)
				rightHandUpState = check2;
			else if(angle < -40)
				rightHandUpState = fail;
			break;
		case check2:
			if(angle > 20 && angle < 40)
			{
				rightHandUpState = success;
			}
			else if(angle < -10)
				rightHandUpState = fail;
			break;
		case success:
			cout << "RIGHT HAND UP!" << endl;
			rightHandUpState = fail;
			break;
		}
	}
}

double Gesture::getRightShoulderHandAngle(NUI_SKELETON_DATA* currentSkeleton)
{
	if(currentSkeleton->eTrackingState == 0 || currentSkeleton->eSkeletonPositionTrackingState[11] == 0 || currentSkeleton->eSkeletonPositionTrackingState[8] == 0 )
		return -200;
	else

		return
		atan(
		(currentSkeleton->SkeletonPositions[11].y - currentSkeleton->SkeletonPositions[8].y) 
		/
		(currentSkeleton->SkeletonPositions[11].x - currentSkeleton->SkeletonPositions[8].x)
		) *  180 / PI
		;
}

void Gesture::proccessLeftHandUpGesture( NUI_SKELETON_DATA* currentSkeleton )
{
	double angle = getLeftShoulderHandAngle(currentSkeleton);

	if(angle != -200)
	{
		switch(leftHandUpState)
		{
		case fail:
			if(angle > -40 && angle < -20)
				leftHandUpState = check1;
			break;
		case check1:
			if(angle > -10 && angle < 10)
				leftHandUpState = check2;
			else if(angle < -40)
				leftHandUpState = fail;
			break;
		case check2:
			if(angle > 20 && angle < 40)
				leftHandUpState = success;
			else if(angle < -10)
				leftHandUpState = fail;
			break;
		case success:
			cout << "LEFT HAND UP!" << endl;
			leftHandUpState = fail;
			break;
		}
	}
}

double Gesture::getLeftShoulderHandAngle(NUI_SKELETON_DATA* currentSkeleton)
{
	if(currentSkeleton->eTrackingState == 0 || currentSkeleton->eSkeletonPositionTrackingState[7] == 0 || currentSkeleton->eSkeletonPositionTrackingState[4] == 0 )
		return -200;
	else

		return
		- atan(
		(currentSkeleton->SkeletonPositions[7].y - currentSkeleton->SkeletonPositions[4].y) 
		/
		(currentSkeleton->SkeletonPositions[7].x - currentSkeleton->SkeletonPositions[4].x)
		) *  180 / PI
		;
}

void Gesture::proccessTwoHandUpGesture( NUI_SKELETON_DATA* currentSkeleton )
{
	double rightAngle = getRightShoulderHandAngle(currentSkeleton);
	double leftAngle = getLeftShoulderHandAngle(currentSkeleton);

	if(leftAngle != -200 && rightAngle != -200)
	{
		switch(twoHandUpState)
		{
		case fail:
			if(rightAngle > -40 && rightAngle < -20 && leftAngle > -40 && leftAngle < -20)
				twoHandUpState = check1;
			break;
		case check1:
			if(rightAngle > -10 && rightAngle < 10 && leftAngle > -10 && leftAngle < 10)
				twoHandUpState = check2;
			else if(rightAngle < -40 || leftAngle < -40)
				twoHandUpState = fail;
			break;
		case check2:
			if(rightAngle > 20 && rightAngle < 40 && leftAngle > 20 && leftAngle < 40)
				twoHandUpState = success;
			else if(rightAngle < -10 || leftAngle < -40)
				twoHandUpState = fail;
			break;
		case success:
			cout << "TWO HAND UP!" << endl;
			twoHandUpState = fail;
			break;
		}
	}
}

int Gesture::getNavigation(NUI_SKELETON_DATA* currentSkeleton)
{
	double rightAngle = getRightShoulderHandAngle(currentSkeleton);
	double leftAngle = getLeftShoulderHandAngle(currentSkeleton);

	int move;

	if(leftAngle == -200 || rightAngle == -200)
		move = NO_HAND;
	else if(rightAngle < 15 && rightAngle > -15 && leftAngle < 15 && leftAngle > -15)
		move = BOTH_HAND;
	else if(rightAngle < 15 && rightAngle > -15)
		move = RIGHT_HAND;
	else if(leftAngle < 15 && leftAngle > -15)
		move = LEFT_HAND;
	else if(rightAngle < -15 && leftAngle > 15)
		move = RIGHT_HAND_MOVE;
	else if(leftAngle < -15 && rightAngle > 15)
		move = LEFT_HAND_MOVE;
	else
		move = NO_HAND;

	cout << move << endl;
	return move;
}